#include <iostream>
#include <cmath>
#include <memory.h>
#include "jsk_recognition_msgs/BoundingBoxArray.h"
#include "std_msgs/Float64MultiArray.h"
#include "geometry_msgs/PoseStamped.h"
#define debug 0
using point = std::pair<double, double>;
point target_point = {90, 0};//终点
double phi = M_PI_2;
constexpr int expected_count = 10;
point center_points[expected_count][16];//0到75线有16对桶
struct control
{
    double v; double delta;
    static constexpr double frequency = 10;//控制频率10Hz
};
struct state
{
    double x; double y; double yaw;
    void update(const control& c)
    {
        x += c.v * std::cos(c.delta) / control::frequency;
        y += c.v * std::sin(c.delta) / control::frequency;
        yaw += c.v * std::tan(c.delta) / control::frequency;
    }
};
// std::vector<point> valid_points;
void lidar_callback(const jsk_recognition_msgs::BoundingBoxArray& lidar)noexcept(true)
{
    static int valid_count = 1;//有效回调（指存在有效数据的回调）次数
    //循环队列的起点为valid_count <= expected_count ? 0 : valid_count % expected_count;
    //本次填写位置(valid_count  - 1) % expected_count
    static int count[16];//每对桶出现的次数，计算平均用
    std::pair<point, point> mate[16];
    memset(mate, 0, sizeof(mate));
    for (const auto& p : lidar.boxes)
    {
#if debug
        std::cout << p.pose.position.x << '\t' << p.pose.position.y << '\n';
#endif
        if (p.pose.position.x > -0.5 and p.pose.position.x < 80)// and std::abs(p.pose.position.y) < 5.0)
        {
            bool not_found = true;
            point* this_time = center_points[valid_count <= expected_count ? 0 : valid_count % expected_count];
            double i = (p.pose.position.x - 0.3) / 5.0;
            int index = std::round(i);
            if (std::abs(i - index) < 0.5)//0.2
            {
                if (mate[index].second.first and mate[index].second.second)
                {
                    if (std::abs(mate[index].second.first - p.pose.position.x) < 0.5 and std::abs(mate[index].second.second - p.pose.position.y) < 0.5)
                    {
                        mate[index].second.first = (mate[index].second.first + p.pose.position.x) / 2.0;
                        mate[index].second.second = (mate[index].second.second + p.pose.position.y) / 2.0;
                    }
                    else if (std::abs(mate[index].first.first - p.pose.position.x) < 0.5 and std::abs(mate[index].first.second - p.pose.position.y) < 0.5)
                    {
                        mate[index].first.first = (mate[index].first.first + p.pose.position.x) / 2.0;
                        mate[index].first.second = (mate[index].first.second + p.pose.position.y) / 2.0;
                    }
                    else
                    {
                        const double abs_p_y = std::abs(p.pose.position.y);
                        if (abs_p_y < std::abs(mate[index].second.second))
                            if (abs_p_y < std::abs(mate[index].first.second))
                            {
                                mate[index].second = mate[index].first;
                                mate[index].first = {p.pose.position.x, p.pose.position.y};
                            }
                            else
                            mate[index].second = {p.pose.position.x, p.pose.position.y};
                    }
                }
                else if (mate[index].first.first and mate[index].first.second)
                {
                    if (std::abs(mate[index].first.first - p.pose.position.x) < 0.5 and std::abs(mate[index].first.second - p.pose.position.y) < 0.5)
                    {
                        mate[index].first.first = (mate[index].first.first + p.pose.position.x) / 2.0;
                        mate[index].first.second = (mate[index].first.second + p.pose.position.y) / 2.0;
                    }
                    else if (std::abs(p.pose.position.y) < std::abs(mate[index].first.second))
                    {
                        mate[index].second = mate[index].first;
                        mate[index].first = {p.pose.position.x, p.pose.position.y};
                    }
                    else
                        mate[index].second = {p.pose.position.x, p.pose.position.y};
                }
                else
                    mate[index].first = {p.pose.position.x, p.pose.position.y};
            }
        }
    }
    int this_time = (valid_count  - 1) % expected_count;
    bool valid = false;
    for (int i = 0;i < 16;++i)
        if (mate[i].first.first and mate[i].first.second and mate[i].second.first and mate[i].second.second)
        {
            valid = true;
            center_points[this_time][i].first = (mate[i].first.first + mate[i].second.first) / 2.0;
            center_points[this_time][i].second = (mate[i].first.second + mate[i].second.second) / 2.0;
        }
    valid_count += valid;
        //         for (auto& each_point: center_points)
        //             if (std::abs(p.pose.position.x -  each_point.first.first) < 0.2 and std::abs(p.pose.position.y - each_point.first.second) < 0.2)
        //             //std::pow(p.pose.position.x - each_point.first.first, 2) + std::pow(p.pose.position.y - each_point.first.second, 2) < 2)//0.25)
        //             {
        //                 each_point.first.first = (each_point.first.first * each_point.second + p.pose.position.x) / (each_point.second + 1);
        //                 each_point.first.second = (each_point.first.second * each_point.second + p.pose.position.y) / (each_point.second + 1);
        //                 ++each_point.second;
        //                 not_found = false;
        //                 break;
        //             }
        //         if (not_found)
        //             points.push_back({{p.pose.position.x, p.pose.position.y}, 1});
        //     }
        // }
//     static int callback_count;//进入回调函数的次数，初始化为0
//     static std::vector<std::pair<std::pair<double, double>, int>> points;//平均点-次数
//     if (points.capacity() < 32)//如果0-75线都能看到，就是32只桶
//         points.reserve(32);
// #if debug
//     std::cout << __func__ << callback_count << '\n';
// #endif
//     if (callback_count < 10)
//     {
//         ++callback_count;

// #if debug
//         for (const auto& p : points)
//             std::cout << p.first.first << '\t' << p.first.second << '\t' << p.second << '\n';
//         std::cout << "--------------------\n";
// #endif
//         return;
}
point last_point()
{
    point average_center_points[16];
    for (int i = 0;i < 16;++i)
    {
        int count = 0;
        for (int times = 0;times < expected_count;++times)
        {
            if (center_points[times][i].first and center_points[times][i].second)
            {
                average_center_points[i].first += center_points[times][i].first;
                average_center_points[i].second += center_points[times][i].second;
                ++count;
            }
        }
        average_center_points[i].first /= count;
        average_center_points[i].second /= count;
    }
    double distance_point[16][16] = {{0.0}};
    for (int i = 0;i < 16;++i)
    {
        for (int j = 0;j < i;++j)
        {
            if (average_center_points[i].first and average_center_points[i].second)
                distance_point[i][j] = std::sqrt(std::pow(average_center_points[i].first - average_center_points[j].first, 2)
                                                + std::pow(average_center_points[i].second - average_center_points[j].second, 2));
        }
    }
    // double distance[16][16][16] = {{{0}}};
    int votes[16] = {0};
    int sum = 0;
    for (int i = 0;i < 16;++i)
    {
        if (average_center_points[i].first == 0 or average_center_points[i].second == 0)
            continue;
        for (int j = 0;j < i;++j)
        {
            if (average_center_points[i].first == 0 or average_center_points[i].second == 0)
                continue;
            for (int k = 0;k < j;++k)
            {
                if (average_center_points[i].first == 0 or average_center_points[i].second == 0)
                    continue;
                sum += 3;
                double p = (distance_point[i][j] + distance_point[i][k] + distance_point[j][k]) / 2.0;
                double double_area = 2.0 * std::sqrt(p * (p - distance_point[i][j]) * (p - distance_point[i][k]) * (p - distance_point[j][k]));
                if (double_area / distance_point[j][k] > 0.5)
                    ++votes[i];
                if (double_area / distance_point[i][k] > 0.5)
                    ++votes[j];
                if (double_area / distance_point[i][j] > 0.5)
                    ++votes[k];
                // distance[i][j][k] = double_area / distance_point[i][j];//k到ij
            }
        }
    }
    int n = 0;
    double sum_xy = 0.0;
    double sum_xx = 0.0;
    double sum_x = 0.0;
    double sum_y = 0.0;
    for (int i = 0;i < 16;++i)
    {
        if (votes[i] / (double)sum < 0.15)
        {
            ++n;
            sum_xy += average_center_points[i].first * average_center_points[i].second;
            sum_xx += average_center_points[i].first * average_center_points[i].first;
            sum_x += average_center_points[i].first;
            sum_y += average_center_points[i].second;
            // valid_points.push_back(average_center_points[i]);
        }
    }
    double k = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);
    target_point.second = k * target_point.first + (sum_y - k  * sum_x) / n;
}
void delta_callback(const std_msgs::Float64MultiArray& ecu)noexcept(true)
{

}
void speed_callback(const geometry_msgs::PoseStamped& pose)noexcept(true)
{
    pose.pose.orientation.w;
}
bool not_ready = true;//初始未收到go
void go_callback(const std_msgs::Float64MultiArray& ecu_go)noexcept(true)
{//ecu_go.data[2]为0时表示还没有go，为1表示go
    not_ready = ecu_go.data[2] < 0.5;//防止精度原因误判
}
int main(int argc, char **argv)noexcept(true)
{
    // valid_points.reserve(16);
    ros::init(argc, argv, "line_path");
    ros::NodeHandle n;
    {//出发前的工作
        ros::Subscriber go(n.subscribe("/ecu_msg", 1, go_callback));//检测go信号
        ros::Subscriber lidar(n.subscribe("/detected_bounding_box", 1, lidar_callback));//雷达消息
        constexpr double phi_limit = 10.0 * M_PI / 180.0;//max(abs(phi))
        //便于用Ctrl + C终止程序；初始航偏角不正常，认为是检测问题/次数没达到；未收到go信号则继续检测
        while (ros::ok() and std::abs(phi) > phi_limit and not_ready)
            ros::spinOnce();
    }//至此不再接收go和雷达消息，两个subscriber自动销毁
    ros::Subscriber delta_actual(n.subscribe("/ecu_msg", 1, delta_callback));//ecu前轮转角
    ros::Subscriber speed_actual(n.subscribe("/current_pose", 1, speed_callback));//惯导速率
    ros::Publisher command(n.advertise<std_msgs::Float64MultiArray>("/command", 1));//控制消息
    

}